#include "UnionSys.hpp"
UnionsysEKF::UnionsysEKF(){
    // EKF Param
    Eigen::VectorXd x0(9);  
    Eigen::MatrixXd P0 = Eigen::MatrixXd::Identity(9, 9) * 0; 
    Eigen::MatrixXd F(9, 9);  
    Eigen::MatrixXd H(3, 9); 
    Eigen::MatrixXd R(3,3);
    Eigen::MatrixXd Q(9,9);
    dt_ = 1/10.7;
    float rconstant = 1.2;
    float qconstant = 0.1;
    x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0; 
    F << 1, 0, 0, dt_, 0, 0, 0.5*dt_*dt_, 0, 0,  
         0, 1, 0, 0, dt_, 0, 0, 0.5*dt_*dt_, 0,
         0, 0, 1, 0, 0, dt_, 0, 0, 0.5*dt_*dt_,
         0, 0, 0, 1, 0, 0, dt_, 0, 0,
         0, 0, 0, 0, 1, 0, 0, dt_, 0,
         0, 0, 0, 0, 0, 1, 0, 0, dt_,
         0, 0, 0, 0, 0, 0, 1, 0, 0,
         0, 0, 0, 0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 0, 0, 0, 1;  
    H << 1, 0, 0, 0, 0, 0, 0, 0, 0,
         0, 1, 0, 0, 0, 0, 0, 0, 0, 
         0, 0, 1, 0, 0, 0, 0, 0, 0; 
    R << rconstant, 0, 0,
         0, rconstant, 0,  
         0, 0, rconstant;
    Q << 10*qconstant, 0, 0, 0, 0, 0, 0, 0, 0,  
         0, 10*qconstant, 0, 0, 0, 0, 0, 0, 0, 
         0, 0, 10*qconstant, 0, 0, 0, 0, 0, 0,  
         0, 0, 0, qconstant, 0, 0, 0, 0, 0,  
         0, 0, 0, 0, qconstant, 0, 0, 0, 0,  
         0, 0, 0, 0, 0, qconstant, 0, 0, 0, 
         0, 0, 0, 0, 0, 0, 0.1*qconstant, 0, 0,
         0, 0, 0, 0, 0, 0, 0, 0.1*qconstant, 0,
         0, 0, 0, 0, 0, 0, 0, 0, 0.1*qconstant;
     x_= x0;
     P_ = P0;
     F_ = F;
     H_ = H;
     R_ = R;
     Q_ = Q; 
}
void UnionsysEKF::predict(){
    x_ = F_ * x_;  
    P_ = F_ * P_ * F_.transpose() + Q_;  
    std::cout<<"---->dt: "<<dt_<<std::endl;
    }
void UnionsysEKF::update(const Eigen::VectorXd& z){
    Eigen::VectorXd y = z - H_ * x_; 
    Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_; 
    K_ = P_ * H_.transpose() * S.inverse();
    x_ = x_ + K_ * y;  
    Eigen::MatrixXd I_KH = (Eigen::MatrixXd::Identity(P_.rows(), P_.rows()) - K_ * H_);
    P_ = I_KH * P_ * I_KH.transpose() + K_ * R_ * K_.transpose();  
    }
